/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file gpsParser.cxx
  \brief class implemenation for gpsParser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "gsBackup.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
extern uint16_t network_id;
/*<-End Global Variables-->*/

GSBackup::GSBackup(String name) : ThreadedPipe(name)
{
}

void GSBackup::update() 
{

	if(me == NULL)
		me = shmPtr->clients.getClient(0);
	if(backup_client == NULL)
	  find_backup_client();
	else {
		if(!backup_client->state.gs_backup)
			backup_client = NULL;
	}
	

	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	maxFD = fifo_FDs[INBOARD_RX];
	if(fifo_FDs[OUTBOARD_RX] > maxFD) maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("GSBackup::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				if(backup_client != NULL) {
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(backup_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
					Command cmd((uint8_t*)fifo_msg.data);
					if(cmd.isValid()) 
						parse(cmd, fifo_msg.src);
					if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
						writeFifo(INBOARD_TX);
					}
					else cout << "INBOARD_TX NOT READY" << endl;
				}
				else { // for stopping connection
					Command cmd((uint8_t*)fifo_msg.data);
					if(cmd.isValid()) 
						if(cmd.cmd() == CMD_ACK)
							writeFifo(INBOARD_TX);
				}
			}
			else cout << "BAD READ on OUTBOARD_RX" << endl;
		}
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				if(backup_client != NULL) {
					Client * a_client = shmPtr->clients.getClient(fifo_msg.dest);
					if(a_client == backup_client) {
						Command cmd((uint8_t*)fifo_msg.data);
						if(cmd.isValid()) 
							parse(cmd, fifo_msg.src);
						if(FD_ISSET(fifo_FDs[OUTBOARD_TX], &writeFDs))
							writeFifo(OUTBOARD_TX);
						else cout << "OUTBOARD_TX NOT READY" << endl;
					} else { // for starting connection
						Command cmd((uint8_t*)fifo_msg.data);
						if(cmd.isValid()) 
							if(cmd.cmd() == CMD_CONNECT)
								writeFifo(OUTBOARD_TX);
					}
				} 
				else { // for stopping connection
					Command cmd((uint8_t*)fifo_msg.data);
					if(cmd.isValid()) 
						if(cmd.cmd() == CMD_CONNECT)
							writeFifo(OUTBOARD_TX);
				}
			}
			else cout << "BAD READ on INBOARD_RX" << endl;
		}
	}
}

void GSBackup::parse(const Command &cmd, in_addr address) 
{
	Client * a_client    = shmPtr->clients.getClient(fifo_msg.src);
	Client * me          = shmPtr->clients.getClient(0);

	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

	switch(cmd.cmd()) {

		case CMD_STREAM_REQ: {
			StreamReq_t * request = (StreamReq_t *)dataPtr;

			if(request->send_data) {
				//if(verbose)
				cout << GREEN << me->name << ATTROFF " -- subscribing " YELLOW << a_client->name 
				     << ATTROFF ":" CYAN << DefinedCommands[request->id].name  << ATTROFF<< endl;
				shmPtr->clients.subscribe(a_client, request->id);
			} else {
				//if(verbose)
				cout << GREEN << me->name << ATTROFF " -- unsubscribing " YELLOW << a_client->name 
				     << ATTROFF ":"  CYAN << DefinedCommands[request->id].name << ATTROFF << endl;
				shmPtr->clients.unsubscribe(a_client, request->id);
			}

		} break;
		case CMD_TELEM: {
      if(a_client->state.gs_telem == 0) a_client->state.gs_telem = 1;
		} break;
		case CMD_FP_SUBS: {
      if(a_client->state.gs_fp == 0) a_client->state.gs_fp = 1;
		} break;
		case CMD_WIND: {
      if(a_client->state.gs_wind == 0) a_client->state.gs_wind = 1;
		} break;

	} // end of command switch
}

void GSBackup::find_backup_client(void) {
	Client * a_client;
	for(int i=1; i<MAX_ADDRESSES; i++){
		a_client = shmPtr->clients.getClient(i);
		if(a_client != NULL)
			if(a_client->state.gs_backup) {
				if( a_client->telemetry.address != 0) {
					backup_client = a_client;
					network_id = a_client->telemetry.address;
					cout << "Found Backup Client: " << a_client->name << endl;
				} else {
					cout <<  "No Piccolo Associated with Backup Client" << endl;
				}
			}
	}
}

